/*
*  iLab@Tongji
*  Author : Yiren Lu 
*  Date : 06/02/2014
*
*/



#include "FreeSixIMU.h"
#include "FIMU_ADXL345.h"
#include "FIMU_ITG3200.h"
#include "HMC5883L.h"
#include "Wire.h"
#include "BMP085.h"

#include "Wire.h"
#include "stdlib.h"
#include "Metro.h"






// all the info we need from Robot
int URMdata[9]={0,0,0,0,0,0,0,0,0};
static float IRdata[7] = { 0,0,0,0,0,0,0 };
int BumperValue =7;
float _speedleft, _speedright;
long Temperature = 0, Pressure = 0, Altitude = 0;
float angles[3];
float heading;








void serialEvent(){
	while(Serial.available()){
		char inChar = (char)Serial.read();
		if(inChar == 'u'){
    			//Serial.print(c);


			for (int i =1 ;i<= 6 ;i++){				
				Serial.print(URMdata[i]);
				Serial.print(" ");
			//int val = URMdata[i];
			//Serial.write(5);
			}
			Serial.print(_speedleft);
			Serial.print(" ");
			Serial.print(_speedright);
			Serial.print(" ");
			for(int i=0;i<7;i++){
				Serial.print(IRdata[i]);
				Serial.print(" ");
			}
			Serial.print(BumperValue);
			Serial.print(" ");
			Serial.println();

		}else if(inChar == 'a'){
			int leftWheelSpeed = Serial.parseInt();
			int rightWheelSpeed = Serial.parseInt();
			advance(leftWheelSpeed, -rightWheelSpeed);

		}

	}

}



















// declarations for IMU

BMP085 dps = BMP085();


// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();

HMC5883L compass;
// Record any errors that may occur in the compass;
int error =0;



//declarations for wheel control

int a, b;

#define Pbn 8
int counter = 0;
const byte encoder0pinA = 2;
const byte encoder0pinB = 4;
const byte encoder1pinA = 3;
const byte encoder1pinB = 5;

byte encoder0PinALast;
byte encoder1PinALast;

int Lduration;
int Rduration;
boolean LDirection;
boolean RDirection;
byte wheelDir = 0x00;
int _CMDspeed[2]={0,0};


float _perimeterA, _FirmPulsePG;
int pastCoder[2];
long totalCoder[2];
#define LF 0
#define RT 1
float _proportion, _integral, _derivative, _maximum, _minimum;
int _speedtarget[2];
double _lasterror[2];
double _preverror[2];
int i;
float _Loutput=0.0, _Routput=0.0;

Metro DataTrans = Metro(1000, true);
Metro BehaviorInterval = Metro(25, true);



//Standard PWM DC control
int E1 = 9;     //M1 Speed Control
int E2 = 10;     //M2 Speed Control
int M1 = 8;    //M1 Direction Control
int M2 = 11;    //M1 Direction Control




int validURM=6;



Metro motorMetro = Metro(500,true);


int count =0;



// The actual speed left and right wheels are not the same under the same speed in code.
int speedleft=25;    
int speedright=25;






void initSpeed(){
	_speedtarget[LF] = speedleft;
	_speedtarget[RT] = -speedright;
}
void advance(char a,char b)          
{
//analogWrite (E1,a);      //PWM Speed Control
//digitalWrite(M1,HIGH);    
//analogWrite (E2,b);    
//digitalWrite(M2,HIGH);
	_speedtarget[LF] = a;
	_speedtarget[RT] = b;
}


void URMreader(){
	Wire.requestFrom(8,9);
	int i=0;
//Serial.print("URM_Data:,");
	while(Wire.available()){
		int c = Wire.read();
		URMdata[i]=c;
		i++;
	}

// from 1 to 6;
	for(int j =1;j<=6;j++){
		int c= URMdata[j];
    //Serial.print(c);
    //Serial.print(",");
	}
//Serial.println();
}



void IRBumperReader(){
	Wire.requestFrom(7,8);
	int i=0;
//Serial.print("IR_Bumper_Data:,");
	while(Wire.available()){
		if(i<7){
			int c = Wire.read();
			IRdata[i]=c;

		}else{
			int c = Wire.read();
			BumperValue = c;
		}
		i++;
	}

	for(i = 0;i<7;i++){
   //Serial.print(IRdata[i]);
   //Serial.print(",");
	}
//Serial.print(BumperValue);
//Serial.print(",");
//Serial.println();

}


void setup(void) 
{ 
	int i;
	for(i=9;i<=11;i++)
		pinMode(i, OUTPUT);  

	for(i=4;i<=7;i++){
		pinMode(i, OUTPUT);
	}
	delay(1000);
	digitalWrite(E1, LOW);
	digitalWrite(E2, LOW);

	initSpeed();


Serial.begin(19200);      //Set Baud Rate
EncoderInit();
_perimeterA = 42.72566*1000;
_FirmPulsePG = 1326;
// PID
_proportion = 3;
_integral = 0.5;
_derivative = 0.6;
_maximum = 500;
_minimum = _maximum*(-1);
i=0;


Wire.begin();
delay(1000);
dps.init();
dps.dumpCalData();
delay(1000); 

delay(5);
sixDOF.init(); // init the Acc and Gyro
delay(5);
compass = HMC5883L();// init HMC5883

error = compass.SetScale(1.3);// Set the scale of the compass
error = compass.SetMeasurementMode(Measurement_Continuous);
if(error!=0){
	Serial.println(compass.GetErrorText(error));
}



} 
int go =0;



void goback(){
	advance(-168,168);
}





void getHeading(){
// Retrive the raw values from the compass (not scaled).
	MagnetometerRaw raw = compass.ReadRawAxis();
// Retrived the scaled values from the compass (scaled to the configured scale).
	MagnetometerScaled scaled = compass.ReadScaledAxis();

// Values are accessed like so:
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

// Calculate heading when the magnetometer is level, then correct for signs of axis.
heading = atan2(scaled.YAxis, scaled.XAxis);  

float declinationAngle = 0.0457;
heading += declinationAngle;

// Correct for when signs are reversed.
if(heading < 0)
	heading += 2*PI;

// Check for wrap due to addition of declination.
if(heading > 2*PI)
	heading -= 2*PI;

// Convert radians to degrees for readability.
heading = heading * 180/M_PI; 
}

void PrintData(){
	Serial.print("Eular Angle: ");
	Serial.print(angles[0]);
	Serial.print("  ");  
	Serial.print(angles[1]);
	Serial.print("  ");
	Serial.print(angles[2]);
	Serial.print("  ");
	Serial.print("Heading: ");
	Serial.print(heading);
	Serial.print("  ");
	Serial.print("Temperature: ");
	Serial.print(Temperature);
	Serial.print("C");
	Serial.print("  ");
	Serial.print("Altitude: ");
	Serial.print(Altitude);
	Serial.print("cm");
	Serial.print("  ");
	Serial.print("Pressure: ");
	Serial.print(Pressure);
	Serial.println(" Pa");
}


void loop(void) 
{

	if(motorMetro.check()){

//read data from IMU

		dps.getTemperature(&Temperature);
		dps.getPressure(&Pressure);
		dps.getAltitude(&Altitude);

		sixDOF.getEuler(angles);
		getHeading();
//PrintData();



      /// Low level control logics:

      //flag 0: forward 1:turn left 2:turn right 3: backup
		int flag =0;
		URMreader();
		IRBumperReader();

/*
Serial.print(_speedtarget[LF]);
  	Serial.print(",");
 	Serial.print(_speedtarget[RT]);
  	Serial.print(",");
  	Serial.print(pastCoder[LF]);
  	Serial.print(",");
  	Serial.print(pastCoder[RT]);
  	Serial.print(",");
  	// output the current wheel speed
  	Serial.print(_speedleft);
  	Serial.print(",");
  	Serial.print(_speedright);
  	Serial.print(",");
  	Serial.print(_Loutput);
  	Serial.print(",");
  	Serial.print(_Routput);
  	Serial.print("\n");
*/



      /*
      for(int i = 2;i<=4;i++){
        if(URMdata[i]<20){
          flag=0; 
        }
      }
      */
      if(URMdata[3]<27||IRdata[2]>200||BumperValue!=7){
         flag=3; //backup
     }
     if(URMdata[2]<27||IRdata[3]>200||IRdata[4]>200){
         flag=1; //turn left
     }
     if(URMdata[6]<27){
        flag=0; //forward
    }
    if(URMdata[4]<27||IRdata[0]>200||IRdata[1]>200){
        flag=2; //turn right
    }


      // This is low level control logic
      // Now we use signals from PC
      //go = flag;
}

if(BehaviorInterval.check()){
	static int lastLspeed = 0;
	static int lastRspeed = 0;
	ResentSpeed();
	static int lastLOutput = 0;
	static int lastROutput = 0;


	float Lpara, Rpara;
// calcuate the targetspeed to the PWM number;
	if(_Loutput ==0||_Routput == 0){
		Lpara = TVPIDcal(_speedleft, true);
		Rpara = TVPIDcal(_speedright, false);
		_Loutput = int(TVAffect(Lpara));
		_Routput = int(TVAffect(Rpara));
	}

	_Loutput += (_speedtarget[LF] - _speedleft);
	_Routput += (_speedtarget[RT] - _speedright);

	Motor(_Loutput, LF);
	Motor(_Routput, RT);

}

  // High level control signals from serial port
  // example : "a 25 25"  (no quotation mark)    
  /*
  if (Serial.available() > 0) {
  	int inByte = Serial.read();
if(inByte == 'a'){
	int leftWheelSpeed = Serial.parseInt();
	int rightWheelSpeed = Serial.parseInt();	
	advance(leftWheelSpeed, -rightWheelSpeed);
}

  }*/

/*
  // do something different depending on the character received.
  // The switch statement expects single number values for each case;
  // in this exmaple, though, you're using single quotes to tell
  // the controller to get the ASCII value for the character.  For
  // example 'a' = 97, 'b' = 98, and so forth:
  switch (inByte) {
  case 'a':
    //Serial.print('a');
    go = 0;
    //forward
    break;
  case 'b':
    //Serial.print('b');
    go = 3;
    //backup
    break;
  case 'l':
    //Serial.print('l');
    go = 1;
    break;
  case 'r':
    //Serial.print('r');
    go = 2;
    break;  
  default:
    break;
  }
*/





	// Low level control logics

  // if you want the robot run autonomously (avoiding obstacles), then umcomment below code:
/*
  if(go ==0){ //forward
    advance (speedleft,-speedright);//forward
  }else if(go==1) { // turn left
    advance(-speedleft,-speedright); //turn left
  }else if(go==2) { // turn right
    advance(speedleft, speedright); //turn right
  }else if(go==3) { // back up
    advance(-speedleft, speedright); //backup
  }
*/  
}

/************************************************************
EncoderInit
This is used for initializing the encoder module
variable:encoder0pinB--the output of the encoder
Pepin
************************************************************/
void EncoderInit() //init of the encoder
{
	LDirection = true;
RDirection = true;//default -> Forward  
pinMode(encoder0pinA,INPUT_PULLUP);
pinMode(encoder0pinB,INPUT_PULLUP); 
pinMode(encoder1pinA,INPUT_PULLUP);
pinMode(encoder1pinB,INPUT_PULLUP);  
attachInterrupt(0, LwheelSpeed, CHANGE);
attachInterrupt(1, RwheelSpeed, CHANGE);
}



/************************************************************
ResentSpeed
This is used for calculating the real time speed of 2 wheels
variable:pasttime-- the past time after the wheels run
       Lduration-- the duration of left wheel
       Rduration--the duration of right wheel
output:_speedleft--the speed of left wheel
     _speedright--the speed of right wheeel
Lauren
************************************************************/
void ResentSpeed()
{
	static int pasttime;
	static unsigned long lasttime;
	static unsigned long now;
	now = millis();
	pasttime = now - lasttime;
	lasttime = now;

	_speedleft = lastspeed(Lduration , pasttime);
	_speedright = lastspeed(Rduration , pasttime);
/* check the encoder */
//  Serial.println(pasttime);
//  Serial.print("  ");
//  Serial.println(Lduration);

	pastCoder[LF] += Lduration;
	pastCoder[RT] += Rduration;

	Lduration = 0;
	Rduration = 0;
}



/************************************************************
wheelSpeed
This is used for reading the value of pulses of the encoder
variable:encoder0pinB and encoder0pin--the output if the sensor
       Direction--the direction of wheel turning
output:duration--the number of the pulses
Pepin
************************************************************/
void LwheelSpeed()  // the encoder code 
{
	int Lstate = digitalRead(encoder0pinA);
	if((encoder0PinALast == LOW) && Lstate==HIGH)
	{
		int val = digitalRead(encoder0pinB);
		if(val == LOW && LDirection)
		{
    LDirection = false; //Reverse
}
else if(val == HIGH && !LDirection)
{
    LDirection = true;  //Forward
}
}
encoder0PinALast = Lstate;

if(!LDirection)  Lduration++;
else  Lduration--;
}

void RwheelSpeed()
{
	int Rstate = digitalRead(encoder1pinA);
	if((encoder1PinALast == LOW) && Rstate==HIGH)
	{
		int val = digitalRead(encoder1pinB);
		if(val == LOW && RDirection)
		{
    RDirection = false; //Reverse
}
else if(val == HIGH && !RDirection)
{
    RDirection = true;  //Forward
}
}
encoder1PinALast = Rstate;

if(!RDirection)  Rduration++;
else  Rduration--;
}


/************************************************ calculate speed (cm/s) ***********************************************/
/************************************************************
lastspeed
This is used for calculating the real time speed
inout:longs--pulses of the encoder
    diff--the times past
variable:_perimeterA--preimter of the wheel
       _FirmPulsePG--total pulses of one circle
output:dist--the speed of the motor
Lauren
************************************************************/
float lastspeed(int longs,int diff)
{
	double internum = _perimeterA;
	internum /= diff;
	float dist = float(internum*longs);
	dist /= _FirmPulsePG;
	return dist;
}




/************************************************* Motor Control ***********************************************/
/************************************************************
Motor
This is used for controlling the motors
inout:value--the PWM value;input of motor
    whichwheel--decide which wheel to control
Pepin
************************************************************/
void Motor(int value,byte whichwheel)
{
	value = constrain(value,1000,2000);

	if(whichwheel == LF)
	{
		if(value>1500)
		{
			a=(value-1500)/1.961;
			analogWrite (E1,a);
			digitalWrite(M1,HIGH);
		}
		else
		{
			a=(1500-value)/2;
			analogWrite (E1,-a);
			digitalWrite(M1,HIGH);
		}
	}
	else if(whichwheel == RT)
	{
		if(value>1500)
		{
			b=(value-1500)/1.961;
			analogWrite (E2,b);
			digitalWrite(M2,HIGH);
		}
		else
		{
			b=(1500-value)/2;
			analogWrite (E2,-b);
			digitalWrite(M2,HIGH);
		}
	}
}

/************************************************* PID Control ***********************************************/

/************************************************************
TVPIDcal
This is used for calculating the output of PID controller
inout:prevspeed--the real time speed
    target--determine whether the first error
variable:_proportion--the proportion constant
       error--the error of the real time speed and the target speed
       _integral--the integral constant
       sumerror[i]--the sum of the errors
       _derivative--the derivative constant
       derror--the derivative of the error
output: TVPIDcal()-- output of the PID controller
Lauren
************************************************************/
float TVPIDcal(float prevspeed,boolean target)
{
	static int sumerror[2];
	static int i;
	if(target)
		i = 0;
	else
		i = 1;
	int derror;
	int error = _speedtarget[i] - prevspeed;

	sumerror[i] += error;
sumerror[i] = min(_maximum,sumerror[i]); //limit the range of intergral segment
sumerror[i] = max(_minimum,sumerror[i]);

derror = _lasterror[i] - _preverror[i];
_preverror[i] = _lasterror[i];
_lasterror[i] = error;

return (_proportion*error+_integral*sumerror[i]+_derivative*derror);
}
/************************************************************
TVAffect
This is used for limiting the output of the PID controller
inout:pidpara--the output of the PID controller
output:TVAffect()
Lauren
************************************************************/
int TVAffect(float pidpara)
{
	float result = 500;
	float factor;

	if(pidpara>_maximum)
		factor = 1;
	else if(pidpara>0)
		factor = pidpara/_maximum;
	else if(pidpara<_minimum)
		factor = -1;
	else
		factor = pidpara/_maximum;

	result *= factor;
	result += 1500;

	return result;
}







